/**
 * File: robot_odom.cpp
 * Brief: this node is work for receive sensor data from mutil-topic, then
 *        build odometry message and transmit it to the odometry topic.
 * Created by: zhangping
 * Created at: 2017.07.10
 * Modified by:
 * Modified at:
 **/


#include "ros/ros.h"
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <tf/transform_broadcaster.h>
#include "nav_msgs/Odometry.h"
#include "robot_msgs/RobotPosition.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Quaternion.h"

// Function handler
typedef const boost::function<void (const geometry_msgs::Twist::ConstPtr& msg)> CallbackVel;
typedef const boost::function<void (const robot_msgs::RobotPosition::ConstPtr& msg)> CallbackPos;
typedef const boost::function<void (const geometry_msgs::Quaternion::ConstPtr& msg)> CallbackOri;


// Callback Functions
void callback_vel(const geometry_msgs::Twist::ConstPtr &msg
    , geometry_msgs::Twist &vel)
{
    vel = *msg;
}

void callback_pos(const robot_msgs::RobotPosition::ConstPtr &msg
    , robot_msgs::RobotPosition &pos)
{
    pos = *msg;
}

void callback_ori(const geometry_msgs::Quaternion::ConstPtr &msg
    , geometry_msgs::Quaternion &quat)
{
    quat = *msg;
}

// Build and publish odometry message
void PubOdom(ros::Publisher &pub,
             const geometry_msgs::Twist &vel,
             const robot_msgs::RobotPosition &pos,
             const geometry_msgs::Quaternion &quat,
             nav_msgs::Odometry &odom_msg)
{
    // Build odometry
    odom_msg.header.stamp = ros::Time::now();

    // Set the position
    odom_msg.header.frame_id = pos.header.frame_id;
    odom_msg.pose.pose.position.x = pos.x;
    odom_msg.pose.pose.position.y = pos.y;
    odom_msg.pose.pose.position.z = 0;
    odom_msg.pose.pose.orientation = quat;

    // Set the velocity
    odom_msg.child_frame_id = "chassis";
    odom_msg.twist.twist = vel;

    // Send odometry
    pub.publish(odom_msg);
}


/**
 * This callback function will build tf message and transform it to "/tf".
 */
void SendTf(const nav_msgs::Odometry& msg
    , tf::TransformBroadcaster &tf_pub)
{
    // computing position
    float px = msg.pose.pose.position.x;
    float py = msg.pose.pose.position.y;
    float pz = msg.pose.pose.position.z;
    float rx = msg.pose.pose.orientation.x;
    float ry = msg.pose.pose.orientation.y;
    float rz = msg.pose.pose.orientation.z;
    float rw = msg.pose.pose.orientation.w;

    // publish to tf
    tf_pub.sendTransform(
        tf::StampedTransform(
            tf::Transform(tf::Quaternion(rx,ry,rz,rw), tf::Vector3(px,py,pz)),
            ros::Time::now(),
            "odom",
            "chassis"));

}


int main(int argc, char **argv)
{
    // Init Node.
    ros::init(argc, argv, "robot_odom_node");
    ros::NodeHandle n;

    // Init global topic
    tf::TransformBroadcaster tf_pub;
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("/robot_odom", 50);
    
    // Subscribe sensor message
    geometry_msgs::Twist vel;
    robot_msgs::RobotPosition pos;
    geometry_msgs::Quaternion quat; quat.w=1;

    CallbackVel vel_callback = boost::bind(callback_vel, _1, boost::ref(vel));
    CallbackPos pos_callback = boost::bind(callback_pos, _1, boost::ref(pos));
    CallbackOri ori_callback = boost::bind(callback_ori, _1, boost::ref(quat));

    ros::Subscriber sub_vel = n.subscribe("robot/sensor/vel", 100, vel_callback);
    ros::Subscriber sub_pos = n.subscribe("robot/sensor/position", 100, pos_callback);
    ros::Subscriber sub_ori = n.subscribe("robot/sensor/Quaternion", 100, ori_callback);


    ros::Rate r(30); // 10 hz
    while(ros::ok()){
        ros::spinOnce();
        r.sleep();

        // send odometry message
        nav_msgs::Odometry odom_msg;
        PubOdom(odom_pub, vel, pos, quat, odom_msg);

        // send tf message
        SendTf(odom_msg, tf_pub);
    }
    
    return 0;
}


